/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include "FleetFormationController.hpp"


static volatile bool run = true;
static ownThreadHandle signalHandlerThread;
static int SignalHandlerThread(void *)
{
  dPrint(8, "Signal handler thread (%ld) starting...", pthread_self());

  while (run) {
    // Now, wait for requested signals to arrive
    int sig;
    sigset_t mask;
    sigemptyset(&mask);
    sigaddset(&mask, SIGINT);
    sigaddset(&mask, SIGTERM);
    sigaddset(&mask, SIGPIPE);
    sigaddset(&mask, SIGHUP);
    sigwait(&mask, &sig);

    // Got signal, sigwait returned!
    dPrint(1,"Got Signal '%d' (%s)", sig, strsignal(sig));

    // What to do?
    switch(sig) {
    case SIGINT:
    case SIGTERM:
    case SIGPIPE:
    case SIGHUP:
      dPrint(1,"Terminating...");
      run = false;
     
      dPrint(1,"Waiting 3 seconds for cleanup...");
      ownSleep_ms(2000);
      dPrint(1,"Exit.");
      ownSleep_ms(1000);
      exit(1); // Force :)
      break;
      
    default:
      dPrint(1,"Unhandled signal! Ignore!");
      break;
    }
  }
  return 0;
}
//*****************************************************************************
//*****************************************************************************

static void InitSignalHandler(void)
{
  // Now; Set to BLOCK ALL SIGNALS
  sigset_t mask;
  sigemptyset(&mask);
  sigaddset(&mask, SIGINT);
  sigaddset(&mask, SIGTERM);
  sigaddset(&mask, SIGPIPE);
  sigaddset(&mask, SIGHUP);
  pthread_sigmask(SIG_BLOCK, &mask, NULL);
  
  // Start signal handler thread
  signalHandlerThread = ownThread_Create((void*)SignalHandlerThread, NULL);
}
//*****************************************************************************
//*****************************************************************************

int main(int argc, char *argv[]) 
{
    // Init Debug lib
  debugInit();
  debugSetGlobalDebugLvl(1);
  debugSetLogFilename("PositionClientExample.log");
  // setup signal handling
  InitSignalHandler();
  // GIMnet parameters
  std::string gimnetAP = "asrobo.hut.fi";
  int gimnetAPPort = 50002;
  std::string gimnetName = "";
  std::string fleetGroup = "FSRSim.";


  std::string masterPositionSourceMaCISL = fleetGroup + "PbV_0.MaCI_Position.Motion";
  std::string slave1CoordinateDriveSourceMaCISL = fleetGroup + "PbV_1.MaCI_CoordinateDrive.Motion";
  std::string slave2CoordinateDriveSourceMaCISL = fleetGroup + "PbV_2.MaCI_CoordinateDrive.Motion";
  static gimi::GIMI g;
  // Objects
  MaCI::Position::CPositionClient masterPC(&g, 0);
  CFleetFormationController controller;
  MaCI::CoordinateDrive::CCoordinateDriveClient slave1CC(&g, 0);
  MaCI::CoordinateDrive::CCoordinateDriveClient slave2CC(&g, 1);
  

  ///Slave's relative positions
  MaCI::Position::TPose2D slave1Pose(0.0,2.0,0.0);
  MaCI::Position::TPose2D slave2Pose(0.0,-2.0,0.0);



  MaCI::EMaCIError e;
  int r;
  if ( (r = g.connectToHubEx(gimnetAP, gimnetAPPort, gimnetName) ) != GIMI_OK) {
    dPrint(1,"Failed to connect to GIMnet AccessPoint '%s:%d' with name '%s': '%s'",
	   gimnetAP.c_str(), 
	   gimnetAPPort, 
	   gimnetName.size()?gimnetName.c_str():"<anonymous>",
	   gimi::getGimiErrorString(r).c_str());
    
  } else {
    dPrint(2,"Succesfully connected to GIMnet at AccessPoint '%s:%d' with name '%s': '%s'",
	   gimnetAP.c_str(), 
	   gimnetAPPort, 
	   gimnetName.size()?gimnetName.c_str():"<anonymous>",
	   gimi::getGimiErrorString(r).c_str());
    
    //First do a service discovery
    gimi::t_serviceList gimiServiceList;
    dPrint(1,"Doing serviceDiscovery");
    if(g.serviceDiscovery(gimiServiceList,5000) != GIMI_OK){
      dPrint(1,"Service discovery failed! Exit!");
      exit(1);
    }


    // GIMI is Open(), so we can execute this.
    MaCI::MaCICtrl::SMaCISL sl(masterPositionSourceMaCISL);
    if (sl.IsValidFQMaCISL()) {
      dPrint(1,"Setting default target to master's position source");
      masterPC.SetDefaultTarget(sl,gimiServiceList);
    }
    MaCI::MaCICtrl::SMaCISL sl1( slave1CoordinateDriveSourceMaCISL);
    if (sl1.IsValidFQMaCISL()) {
      dPrint(1,"Setting default target to slave1 coordinateDrive source");
      slave1CC.SetDefaultTarget(sl1,gimiServiceList);
    }
    MaCI::MaCICtrl::SMaCISL sl2( slave2CoordinateDriveSourceMaCISL);
    if (sl2.IsValidFQMaCISL()) {
      dPrint(1,"Setting default target to slave2 coordinateDrive source");
      slave2CC.SetDefaultTarget(sl2, gimiServiceList);
    }
   
    // Attempt to Open Client
    if ( (e = masterPC.Open()) != MaCI::KMaCIOK) {
      dPrint(1,"Failed to Open Master's PositionClient instance! (%s)",
	     GetErrorStr(e).c_str());
    } else {
      if ( (e = slave1CC.Open()) != MaCI::KMaCIOK) {
        dPrint(1,"Failed to Open Slave 1 CoordinateDriveClient instance! (%s)",
               GetErrorStr(e).c_str());
      }else{
        if ( (e = slave2CC.Open()) != MaCI::KMaCIOK) {
        dPrint(1,"Failed to Open Slave 1 CoordinateDriveClient instance! (%s)",
               GetErrorStr(e).c_str());
        }else{
          dPrint(1,"Clients opened, setting the machines");
          //Everything is open, the set the "machines"
          controller.SetMasterMachine(&masterPC);
          
          controller.AddSlaveMachine(&slave1CC,
                                     slave1Pose);       

          controller.AddSlaveMachine(&slave2CC,
                                     slave2Pose);
          dPrint(1,"Going to formation");
          controller.GoToFormation();
          ownSleep_ms(1000);
          dPrint(1,"Start to follow");
          controller.StartToFollow();

          while(run) {
            
            ownSleep_ms(1000);
          }
          controller.StopToFollow();
        }
      }
    }
  }
  dPrint(1,"Exited program..");

  exit(1);
}
